/*
 * task_transfer.c
 *
 *  Created on: 2025 Jul 6
 *      Author: SFLYs
 */
#include "task_transfer.h"

xt_hal_canmessage_t FLeg_RecvMessages,RLeg_RecvMessages;
xt_hal_canmessage_t FLeg_SendMessages,RLeg_SendMessages;

xt_hal_canmessage_t FL_Hip_Msgs,FL_Thigh_Msgs,FL_Calf_Msgs;
xt_hal_canmessage_t FR_Hip_Msgs,FR_Thigh_Msgs,FR_Calf_Msgs;

xt_hal_canmessage_t RL_Hip_Msgs,RL_Thigh_Msgs,RL_Calf_Msgs;
xt_hal_canmessage_t RR_Hip_Msgs,RR_Thigh_Msgs,RR_Calf_Msgs;



//leg_state_t FL_state, FR_state;
Leg_State_t FLeg_State;
leg_control_t FL_control,FR_control;

//leg_state_t RL_state, RR_state;
Leg_State_t RLeg_State;
leg_control_t RL_control,RR_control;

spi_data_t    spi_data;     // data from spine to up
spi_command_t spi_command;  // data from up to spine

// spi buffers
__attribute__((aligned(4))) uint16_t rx_buff[RX_LEN];
__attribute__((aligned(4))) uint16_t tx_buff[TX_LEN];

int enabled = 0;

void Task_Board_Init(void)
{
    Bsp_CANFD_Init();
    Bsp_SPI_Init();
    Bsp_Led_Drv_Init(FLeg_Status_LED);
    Bsp_Led_Drv_Init(RLeg_Status_LED);
    Bsp_Led_Drv_Init(PC_Status_LED);
//    Bsp_Led_Drv_Control(FLeg_Status_LED,LED_ON);
//    Bsp_Led_Drv_Control(RLeg_Status_LED,LED_ON);
//    memset(&tx_buff, 0, TX_LEN * sizeof(uint16_t));
//    memset(&spi_data, 0, sizeof(spi_data_t));
//    memset(&spi_command,0,sizeof(spi_command_t));
//
    FL_Hip_Msgs.can_id = 0x01;
    FL_Thigh_Msgs.can_id = 0x02;
    FL_Calf_Msgs.can_id = 0x03;
    FR_Hip_Msgs.can_id = 0x04;
    FR_Thigh_Msgs.can_id = 0x05;
    FR_Calf_Msgs.can_id = 0x06;

    RL_Hip_Msgs.can_id = 0x01;
    RL_Thigh_Msgs.can_id = 0x02;
    RL_Calf_Msgs.can_id = 0x03;
    RR_Hip_Msgs.can_id = 0x04;
    RR_Thigh_Msgs.can_id = 0x05;
    RR_Calf_Msgs.can_id = 0x06;

    Task_Pack_Cmd(&FL_Hip_Msgs, FL_control.hip);
    Task_Pack_Cmd(&FL_Thigh_Msgs, FL_control.thigh);
    Task_Pack_Cmd(&FL_Calf_Msgs, FL_control.calf);

    Task_Pack_Cmd(&FR_Hip_Msgs, FR_control.hip);
    Task_Pack_Cmd(&FR_Thigh_Msgs, FR_control.thigh);
    Task_Pack_Cmd(&FR_Calf_Msgs, FR_control.calf);

    Task_Pack_Cmd(&RL_Hip_Msgs, RL_control.hip);
    Task_Pack_Cmd(&RL_Thigh_Msgs, RL_control.thigh);
    Task_Pack_Cmd(&RL_Calf_Msgs, RL_control.calf);

    Task_Pack_Cmd(&RR_Hip_Msgs, RR_control.hip);
    Task_Pack_Cmd(&RR_Thigh_Msgs, RR_control.thigh);
    Task_Pack_Cmd(&RR_Calf_Msgs, RR_control.calf);
    Task_WriteAll();
    for(int i=0;i<RX_LEN;i++)
    {
        rx_buff[i] = 0;
        tx_buff[i] = 2;
    }

}


void Task_Pack_Cmd(xt_hal_canmessage_t * msg, joint_control_t joint){

     /// limit data to be within bounds ///
     float p_des = fminf(fmaxf(P_MIN, joint.p_des), P_MAX);
     float v_des = fminf(fmaxf(V_MIN, joint.v_des), V_MAX);
     float kp = fminf(fmaxf(KP_MIN, joint.kp), KP_MAX);
     float kd = fminf(fmaxf(KD_MIN, joint.kd), KD_MAX);
     float t_ff = fminf(fmaxf(T_MIN, joint.t_ff), T_MAX);
     /// convert floats to unsigned ints ///
     uint16_t p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);
     uint16_t v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
     uint16_t kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
     uint16_t kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
     uint16_t t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
     /// pack ints into the can buffer ///
     msg->candata[0] = p_int>>8;
     msg->candata[1] = p_int&0xFF;
     msg->candata[2] = v_int>>4;
     msg->candata[3] = ((v_int&0xF)<<4)|(kp_int>>8);
     msg->candata[4] = kp_int&0xFF;
     msg->candata[5] = kd_int>>4;
     msg->candata[6] = ((kd_int&0xF)<<4)|(t_int>>8);
     msg->candata[7] = t_int&0xff;
     }

/// CAN Reply Packet Structure ///
/// 16 bit position, between -4*pi and 4*pi
/// 12 bit velocity, between -30 and + 30 rad/s
/// 12 bit current, between -40 and 40;
/// CAN Packet is 5 8-bit words
/// Formatted as follows.  For each quantity, bit 0 is LSB
/// 0: [position[15-8]]
/// 1: [position[7-0]]
/// 2: [velocity[11-4]]
/// 3: [velocity[3-0], current[11-8]]
/// 4: [current[7-0]]

void Task_Unpack_Reply(xt_hal_canmessage_t msg, Leg_State_t * leg){
    /// unpack ints from can buffer ///
    uint16_t id = msg.candata[0];
    uint16_t p_int = (msg.candata[1]<<8)|msg.candata[2];
    uint16_t v_int = (msg.candata[3]<<4)|(msg.candata[4]>>4);
    uint16_t t_int = ((msg.candata[4]&0xF)<<8)|msg.candata[5];
    /// convert uints to floats ///
    float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
    float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
    float t = uint_to_float(t_int, -T_MAX, T_MAX, 12);

    if(id==1){
        leg->L_Leg_state.hip.p = p;
        leg->L_Leg_state.hip.v = v;
        leg->L_Leg_state.hip.t = t;
        }
    else if(id==2){
        leg->L_Leg_state.thigh.p = p;
        leg->L_Leg_state.thigh.v = v;
        leg->L_Leg_state.thigh.t = t;
        }
    else if(id==3){
        leg->L_Leg_state.calf.p = p;
        leg->L_Leg_state.calf.v = v;
        leg->L_Leg_state.calf.t = t;
        }
    else if(id==4){
        leg->R_Leg_state.thigh.p = p;
        leg->R_Leg_state.thigh.v = v;
        leg->R_Leg_state.thigh.t = t;
        }
    else if(id==5){
        leg->R_Leg_state.calf.p = p;
        leg->R_Leg_state.calf.v = v;
        leg->R_Leg_state.calf.t = t;
        }
    else if(id==6){
        leg->R_Leg_state.thigh.p = p;
        leg->R_Leg_state.thigh.v = v;
        leg->R_Leg_state.thigh.t = t;
        }
    }


void Task_PackAll(void){
    Task_Pack_Cmd(&FL_Hip_Msgs, FL_control.hip);
    Task_Pack_Cmd(&FL_Thigh_Msgs, FL_control.thigh);
    Task_Pack_Cmd(&FL_Calf_Msgs, FL_control.calf);

    Task_Pack_Cmd(&FR_Hip_Msgs, FR_control.hip);
    Task_Pack_Cmd(&FR_Thigh_Msgs, FR_control.thigh);
    Task_Pack_Cmd(&FR_Calf_Msgs, FR_control.calf);


    Task_Pack_Cmd(&RL_Hip_Msgs, RL_control.hip);
    Task_Pack_Cmd(&RL_Thigh_Msgs, RL_control.thigh);
    Task_Pack_Cmd(&RL_Calf_Msgs, RL_control.calf);

    Task_Pack_Cmd(&RR_Hip_Msgs, RR_control.hip);
    Task_Pack_Cmd(&RR_Thigh_Msgs, RR_control.thigh);
    Task_Pack_Cmd(&RR_Calf_Msgs, RR_control.calf);
}

void Task_WriteAll(void){

    XT_Hal_CanFD_SendMessage(XT_HAL_FLeg_CAN,&FL_Hip_Msgs);
    XT_Hal_CanFD_SendMessage(XT_HAL_FLeg_CAN,&FL_Thigh_Msgs);
    XT_Hal_CanFD_SendMessage(XT_HAL_FLeg_CAN,&FL_Calf_Msgs);
    XT_Hal_CanFD_SendMessage(XT_HAL_FLeg_CAN,&FR_Hip_Msgs);
    XT_Hal_CanFD_SendMessage(XT_HAL_FLeg_CAN,&FR_Thigh_Msgs);
    XT_Hal_CanFD_SendMessage(XT_HAL_FLeg_CAN,&FR_Calf_Msgs);

    XT_Hal_CanFD_SendMessage(XT_HAL_RLeg_CAN,&RL_Hip_Msgs);
    XT_Hal_CanFD_SendMessage(XT_HAL_RLeg_CAN,&RL_Thigh_Msgs);
    XT_Hal_CanFD_SendMessage(XT_HAL_RLeg_CAN,&RL_Calf_Msgs);
    XT_Hal_CanFD_SendMessage(XT_HAL_RLeg_CAN,&RR_Hip_Msgs);
    XT_Hal_CanFD_SendMessage(XT_HAL_RLeg_CAN,&RR_Thigh_Msgs);
    XT_Hal_CanFD_SendMessage(XT_HAL_RLeg_CAN,&RR_Calf_Msgs);

}



void Task_EnterMotorMode(xt_hal_canmessage_t * msg){
    msg->candata[0] = 0xFF;
    msg->candata[1] = 0xFF;
    msg->candata[2] = 0xFF;
    msg->candata[3] = 0xFF;
    msg->candata[4] = 0xFF;
    msg->candata[5] = 0xFF;
    msg->candata[6] = 0xFF;
    msg->candata[7] = 0xFC;
    }

void Task_ExitMotorMode(xt_hal_canmessage_t * msg){
    msg->candata[0] = 0xFF;
    msg->candata[1] = 0xFF;
    msg->candata[2] = 0xFF;
    msg->candata[3] = 0xFF;
    msg->candata[4] = 0xFF;
    msg->candata[5] = 0xFF;
    msg->candata[6] = 0xFF;
    msg->candata[7] = 0xFD;
    }

uint32_t xor_checksum(uint32_t* data, size_t len)
{
    uint32_t t = 0;
    for(int i = 0; i < len; i++)
        t = t ^ data[i];
    return t;
}


int Task_softstop_joint(joint_state_t state, joint_control_t * control, float limit_p, float limit_n){
    if((state.p)>=limit_p){
        //control->p_des = limit_p;
        control->v_des = 0.0f;
        control->kp = 0;
        control->kd = KD_SOFTSTOP;
        control->t_ff += KP_SOFTSTOP*(limit_p - state.p);
        return 1;
    }
    else if((state.p)<=limit_n){
        //control->p_des = limit_n;
        control->v_des = 0.0f;
        control->kp = 0;
        control->kd = KD_SOFTSTOP;
        control->t_ff += KP_SOFTSTOP*(limit_n - state.p);
        return 1;
    }
    return 0;

    }

void Task_control(void)
{

    if(((spi_command.flags[0]&0x1)==1)  && (enabled==0)){
        enabled = 1;
        Task_EnterMotorMode(&FL_Hip_Msgs);
        XT_Hal_CanFD_SendMessage(XT_HAL_FLeg_CAN,&FL_Hip_Msgs);
        Task_EnterMotorMode(&FL_Thigh_Msgs);
        XT_Hal_CanFD_SendMessage(XT_HAL_FLeg_CAN,&FL_Thigh_Msgs);
        Task_EnterMotorMode(&FL_Calf_Msgs);
        XT_Hal_CanFD_SendMessage(XT_HAL_FLeg_CAN,&FL_Calf_Msgs);

        Task_EnterMotorMode(&FR_Hip_Msgs);
        XT_Hal_CanFD_SendMessage(XT_HAL_FLeg_CAN,&FR_Hip_Msgs);
        Task_EnterMotorMode(&FR_Thigh_Msgs);
        XT_Hal_CanFD_SendMessage(XT_HAL_FLeg_CAN,&FR_Thigh_Msgs);
        Task_EnterMotorMode(&FR_Calf_Msgs);
        XT_Hal_CanFD_SendMessage(XT_HAL_FLeg_CAN,&FR_Calf_Msgs);


        Task_EnterMotorMode(&RL_Hip_Msgs);
        XT_Hal_CanFD_SendMessage(XT_HAL_RLeg_CAN,&RL_Hip_Msgs);
        Task_EnterMotorMode(&RL_Thigh_Msgs);
        XT_Hal_CanFD_SendMessage(XT_HAL_RLeg_CAN,&RL_Thigh_Msgs);
        Task_EnterMotorMode(&RL_Calf_Msgs);
        XT_Hal_CanFD_SendMessage(XT_HAL_RLeg_CAN,&RL_Calf_Msgs);

        Task_EnterMotorMode(&RR_Hip_Msgs);
        XT_Hal_CanFD_SendMessage(XT_HAL_RLeg_CAN,&RR_Hip_Msgs);
        Task_EnterMotorMode(&RR_Thigh_Msgs);
        XT_Hal_CanFD_SendMessage(XT_HAL_RLeg_CAN,&RR_Thigh_Msgs);
        Task_EnterMotorMode(&RR_Calf_Msgs);
        XT_Hal_CanFD_SendMessage(XT_HAL_RLeg_CAN,&RR_Calf_Msgs);
        return;
    }
    else if((((spi_command.flags[0]&0x1))==0)  && (enabled==1)){
         enabled = 0;
         Task_ExitMotorMode(&FL_Hip_Msgs);
         XT_Hal_CanFD_SendMessage(XT_HAL_FLeg_CAN,&FL_Hip_Msgs);
         Task_ExitMotorMode(&FL_Thigh_Msgs);
         XT_Hal_CanFD_SendMessage(XT_HAL_FLeg_CAN,&FL_Thigh_Msgs);
         Task_ExitMotorMode(&FL_Calf_Msgs);
         XT_Hal_CanFD_SendMessage(XT_HAL_FLeg_CAN,&FL_Calf_Msgs);

         Task_ExitMotorMode(&FR_Hip_Msgs);
         XT_Hal_CanFD_SendMessage(XT_HAL_FLeg_CAN,&FR_Hip_Msgs);
         Task_ExitMotorMode(&FR_Thigh_Msgs);
         XT_Hal_CanFD_SendMessage(XT_HAL_FLeg_CAN,&FR_Thigh_Msgs);
         Task_ExitMotorMode(&FR_Calf_Msgs);
         XT_Hal_CanFD_SendMessage(XT_HAL_FLeg_CAN,&FR_Calf_Msgs);


         Task_ExitMotorMode(&RL_Hip_Msgs);
         XT_Hal_CanFD_SendMessage(XT_HAL_RLeg_CAN,&RL_Hip_Msgs);
         Task_ExitMotorMode(&RL_Thigh_Msgs);
         XT_Hal_CanFD_SendMessage(XT_HAL_RLeg_CAN,&RL_Thigh_Msgs);
         Task_ExitMotorMode(&RL_Calf_Msgs);
         XT_Hal_CanFD_SendMessage(XT_HAL_RLeg_CAN,&RL_Calf_Msgs);

         Task_ExitMotorMode(&RR_Hip_Msgs);
         XT_Hal_CanFD_SendMessage(XT_HAL_RLeg_CAN,&RR_Hip_Msgs);
         Task_ExitMotorMode(&RR_Thigh_Msgs);
         XT_Hal_CanFD_SendMessage(XT_HAL_RLeg_CAN,&RR_Thigh_Msgs);
         Task_ExitMotorMode(&RR_Calf_Msgs);
         XT_Hal_CanFD_SendMessage(XT_HAL_RLeg_CAN,&RR_Calf_Msgs);
         return;
        }


    spi_data.q_hip[0]   = FLeg_State.L_Leg_state.hip.p;
    spi_data.q_thigh[0] = FLeg_State.L_Leg_state.thigh.p;
    spi_data.q_calf[0]  = FLeg_State.L_Leg_state.calf.p;

    spi_data.q_hip[1]   = FLeg_State.R_Leg_state.hip.p;
    spi_data.q_thigh[1] = FLeg_State.R_Leg_state.thigh.p;
    spi_data.q_calf[1]  = FLeg_State.R_Leg_state.calf.p;

    spi_data.q_hip[2]   = RLeg_State.L_Leg_state.hip.p;
    spi_data.q_thigh[2] = RLeg_State.L_Leg_state.thigh.p;
    spi_data.q_calf[2]  = RLeg_State.L_Leg_state.calf.p;

    spi_data.q_hip[3]   = RLeg_State.R_Leg_state.hip.p;
    spi_data.q_thigh[3] = RLeg_State.R_Leg_state.thigh.p;
    spi_data.q_calf[3]  = RLeg_State.R_Leg_state.calf.p;

    spi_data.v_hip[0]   = FLeg_State.L_Leg_state.hip.v;
    spi_data.v_thigh[0] = FLeg_State.L_Leg_state.thigh.v;
    spi_data.v_calf[0]  = FLeg_State.L_Leg_state.calf.v;

    spi_data.v_hip[1]   = FLeg_State.R_Leg_state.hip.v;
    spi_data.v_thigh[1] = FLeg_State.R_Leg_state.thigh.v;
    spi_data.v_calf[1]  = FLeg_State.R_Leg_state.calf.v;

    spi_data.v_hip[2]   = RLeg_State.L_Leg_state.hip.v;
    spi_data.v_thigh[2] = RLeg_State.L_Leg_state.thigh.v;
    spi_data.v_calf[2]  = RLeg_State.L_Leg_state.calf.v;

    spi_data.v_hip[3]   = RLeg_State.R_Leg_state.hip.v;
    spi_data.v_thigh[3] = RLeg_State.R_Leg_state.thigh.v;
    spi_data.v_calf[3]  = RLeg_State.R_Leg_state.calf.v;




    if(SPI_GPIO_CS_ESTOP==0){
        memset(&FL_control,0,sizeof(FL_control));
        memset(&FR_control,0,sizeof(FR_control));
        memset(&RL_control,0,sizeof(RL_control));
        memset(&RR_control,0,sizeof(RR_control));
        spi_data.flags[0] = 0xdead;
        spi_data.flags[1] = 0xdead;
        spi_data.flags[2] = 0xdead;
        spi_data.flags[3] = 0xdead;
        }
    else{
        memset(&FL_control,0,sizeof(FL_control));
        memset(&FR_control,0,sizeof(FR_control));
        memset(&RL_control,0,sizeof(RL_control));
        memset(&RR_control,0,sizeof(RR_control));
        FL_control.hip.p_des = spi_command.q_des_hip[0];
        FL_control.hip.v_des = spi_command.v_des_hip[0];
        FL_control.hip.t_ff = spi_command.tau_hip_ff[0];
        FL_control.hip.kp =  spi_command.kp_hip[0];
        FL_control.hip.kd = spi_command.kd_hip[0];

        FR_control.hip.p_des = spi_command.q_des_hip[1];
        FR_control.hip.v_des = spi_command.v_des_hip[1];
        FR_control.hip.t_ff = spi_command.tau_hip_ff[1];
        FR_control.hip.kp =  spi_command.kp_hip[1];
        FR_control.hip.kd = spi_command.kd_hip[1];

        RL_control.hip.p_des = spi_command.q_des_hip[2];
        RL_control.hip.v_des = spi_command.v_des_hip[2];
        RL_control.hip.t_ff = spi_command.tau_hip_ff[2];
        RL_control.hip.kp =  spi_command.kp_hip[2];
        RL_control.hip.kd = spi_command.kd_hip[2];


        RR_control.hip.p_des = spi_command.q_des_hip[3];
        RR_control.hip.v_des = spi_command.v_des_hip[3];
        RR_control.hip.t_ff = spi_command.tau_hip_ff[3];
        RR_control.hip.kp =  spi_command.kp_hip[3];
        RR_control.hip.kd = spi_command.kd_hip[3];


        FL_control.thigh.p_des = spi_command.q_des_thigh[0];
        FL_control.thigh.v_des = spi_command.v_des_thigh[0];
        FL_control.thigh.t_ff = spi_command.tau_thigh_ff[0];
        FL_control.thigh.kp =  spi_command.kp_thigh[0];
        FL_control.thigh.kd = spi_command.kd_thigh[0];

        FR_control.thigh.p_des = spi_command.q_des_thigh[1];
        FR_control.thigh.v_des = spi_command.v_des_thigh[1];
        FR_control.thigh.t_ff = spi_command.tau_thigh_ff[1];
        FR_control.thigh.kp =  spi_command.kp_thigh[1];
        FR_control.thigh.kd = spi_command.kd_thigh[1];

        RL_control.thigh.p_des = spi_command.q_des_thigh[2];
        RL_control.thigh.v_des = spi_command.v_des_thigh[2];
        RL_control.thigh.t_ff = spi_command.tau_thigh_ff[2];
        RL_control.thigh.kp =  spi_command.kp_thigh[2];
        RL_control.thigh.kd = spi_command.kd_thigh[2];


        RR_control.thigh.p_des = spi_command.q_des_thigh[3];
        RR_control.thigh.v_des = spi_command.v_des_thigh[3];
        RR_control.thigh.t_ff = spi_command.tau_thigh_ff[3];
        RR_control.thigh.kp =  spi_command.kp_thigh[3];
        RR_control.thigh.kd = spi_command.kd_thigh[3];


        FL_control.calf.p_des = spi_command.q_des_calf[0];
        FL_control.calf.v_des = spi_command.v_des_calf[0];
        FL_control.calf.t_ff = spi_command.tau_calf_ff[0];
        FL_control.calf.kp =  spi_command.kp_calf[0];
        FL_control.calf.kd = spi_command.kd_calf[0];

        FR_control.calf.p_des = spi_command.q_des_calf[1];
        FR_control.calf.v_des = spi_command.v_des_calf[1];
        FR_control.calf.t_ff = spi_command.tau_calf_ff[1];
        FR_control.calf.kp =  spi_command.kp_calf[1];
        FR_control.calf.kd = spi_command.kd_calf[1];

        RL_control.calf.p_des = spi_command.q_des_calf[2];
        RL_control.calf.v_des = spi_command.v_des_calf[2];
        RL_control.calf.t_ff = spi_command.tau_calf_ff[2];
        RL_control.calf.kp =  spi_command.kp_calf[2];
        RL_control.calf.kd = spi_command.kd_calf[2];


        RR_control.calf.p_des = spi_command.q_des_calf[3];
        RR_control.calf.v_des = spi_command.v_des_calf[3];
        RR_control.calf.t_ff = spi_command.tau_calf_ff[3];
        RR_control.calf.kp =  spi_command.kp_calf[3];
        RR_control.calf.kd = spi_command.kd_calf[3];



        spi_data.flags[0] = 0;
        spi_data.flags[1] = 0;
        spi_data.flags[2] = 0;
        spi_data.flags[3] = 0;
        spi_data.flags[0] |= Task_softstop_joint(FLeg_State.L_Leg_state.hip, &FL_control.hip, A_LIM_P, A_LIM_N);
        spi_data.flags[0] |= (Task_softstop_joint(FLeg_State.L_Leg_state.thigh, &FL_control.thigh, H_LIM_P, H_LIM_N))<<1;
        spi_data.flags[1] |= Task_softstop_joint(FLeg_State.R_Leg_state.hip, &FR_control.hip, A_LIM_P, A_LIM_N);
        spi_data.flags[1] |= (Task_softstop_joint(FLeg_State.R_Leg_state.thigh, &FR_control.thigh, H_LIM_P, H_LIM_N))<<1;
        spi_data.flags[2] |= Task_softstop_joint( RLeg_State.L_Leg_state.hip, &RL_control.hip, A_LIM_P, A_LIM_N);
        spi_data.flags[2] |= (Task_softstop_joint( RLeg_State.L_Leg_state.thigh, &RL_control.thigh, H_LIM_P, H_LIM_N))<<1;
        spi_data.flags[3] |= Task_softstop_joint(RLeg_State.R_Leg_state.hip, &RR_control.hip, A_LIM_P, A_LIM_N);
        spi_data.flags[3] |= (Task_softstop_joint(RLeg_State.R_Leg_state.thigh, &RR_control.thigh, H_LIM_P, H_LIM_N))<<1;
    }
    spi_data.checksum = xor_checksum((uint32_t*)&spi_data,28);
    for(int i = 0; i < DATA_LEN; i++){
        tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
}


uint32_t calc_checksum;
__interrupt void spiISR(void)
{
    int bytecount = 0;
    while(SPI_GPIO_CS_STATUS==0)
    {
        while (SPI_getRxFIFOStatus((u32)&SpibRegs.SLAVE) != SPI_FIFO_RXEMPTY)
          {
            rx_buff[bytecount++] =SPI_readDataNonBlocking((u32)&SpibRegs.SLAVE);
            if(bytecount<TX_LEN) {
                SPI_writeDataNonBlocking((u32)&SpibRegs.SLAVE,tx_buff[bytecount]);
            }
          }
    }
    calc_checksum = xor_checksum((uint32_t*)rx_buff,64);
    for(int i = 0; i < CMD_LEN; i++)
    {
       ((uint16_t*)(&spi_command))[i] = rx_buff[i];
    }
    if(calc_checksum != spi_command.checksum){
        Bsp_Led_Drv_Control(FLeg_Status_LED,LED_ON);
        spi_data.flags[1] = 0xdead;}
    else
    {
        Bsp_Led_Drv_Control(FLeg_Status_LED,LED_OFF);
    }
    Task_control();
    Task_PackAll();
    Task_WriteAll();
}

void Task_LowPriorityEvent(void)
{
    XT_Hal_CANFD_RecvMessage(XT_HAL_FLeg_CAN,&FLeg_RecvMessages);//接收前腿数据总线数据
    Task_Unpack_Reply(FLeg_RecvMessages,&FLeg_State);
    XT_Hal_CANFD_RecvMessage(XT_HAL_RLeg_CAN,&RLeg_RecvMessages);//接收后腿数据总线数据
    Task_Unpack_Reply(RLeg_RecvMessages,&RLeg_State);
}

